%a private function for calcSnake to perform the minimization.
% n - the index to the 
%neighborhoodSize - 
%Author: Jason Heidemann 
function [newX,newY] = MinimizeE(rp,cp,n,gradMag,neighborhoodSize,d);

N = size(rp,1); %number of points in snake
halfSize = floor(neighborhoodSize/2);

%get local points around a snake vertex, will be used for a local min search
[possR,possC] = meshgrid(rp(n)-halfSize:rp(n)+halfSize,cp(n)-halfSize:cp(n)+halfSize);

%reshape to column vectors
possR = reshape(possR,neighborhoodSize^2,1);
possC = reshape(possC,neighborhoodSize^2,1);

d = [rp-circshift(rp,1) cp-circshift(cp,1)]; %first point will always be zero because of wrap around
d = sqrt((sum(d.^2,2))); %L2 distance between points on the snake
%meanD = mean(d(2:N));    %get the average value

keyboard
%we want the previous point, but the first point is special because it is duplicated 
%as the last point in the array so to get the previous point before the first we really need the true last 
%point we need the second to last in the array.
if n==1 
    nAR = rp(N-1);
    nAC = cp(N-1);
else
    nAR = rp(n-1);
    nAC = cp(n-1);
end

%get the next point
nBR = rp(n+1);
nBC = cp(n+1);

%newDA = sqrt( (possR - nAR).^2 + (possC - nAC).^2 );
%newDB = sqrt( (possR - nBR).^2 + (possC - nBC).^2 );

% %Econt = sqrt((newDA - meanD).^2 + (newDB - meanD).^2);
% Econt = (newDA - meanD).^2 + (newDB - meanD).^2;
% nEcont = Econt / max(Econt);


%Implement the greedy method
for j = 1:size(possR,1);
    temprp = rp;
    temprp(n) = possR(j);
    tempcp = cp;
    tempcp(n) = possC(j);
    d = [temprp-circshift(temprp,1) tempcp-circshift(tempcp,1)];
    d = sqrt((sum(d.^2,2))); %L2 distance between points on the snake
    %meanD = mean(d(2:N));
    varD = var(d(2:N));
    Econt(j) = varD;
end
nEcont = Econt.'/max(Econt);

Ecurv = (nAR + nBR - 2*possR).^2 + (nAC + nBC - 2*possC).^2;

if n==2
    nA2R = rp(N-1);
    nA2C = cp(N-1);
else
    if n==1
        nA2R = rp(N-2);
        nA2C = cp(N-2);
    else
        nA2R = rp(n-2);
        nA2C = cp(n-1);
    end
end
Ecurv = Ecurv + (nA2R + possR - 2*nAR).^2 + (nA2C + possC - 2*nAC).^2;
if n==N-1
    nB2R = rp(2);
    nB2C = cp(2);
else
    nB2R = rp(n+2);
    nB2C = cp(n+2);
end
Ecurv = Ecurv + (nB2R + possR - 2*nBR).^2 + (nB2C + possC - 2*nBC).^2;

nEcurv = Ecurv / max(Ecurv);

Eimage = gradMag(sub2ind(size(gradMag),possR,possC));
nEimage = -(Eimage - min(Eimage))/(max(Eimage) - min(Eimage)+eps);

a = 3; 
b = 1; 
g = 1.2;


newE = a*nEcont + b*nEcurv + g*nEimage;
best = find(newE == min(newE));
if ~isempty(best)
    newX = possR(best(1));
    newY = possC(best(1));
else
    newX = rp(n);
    newY = cp(n);
end

disp('');

